#include "radar_tracking.h"
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
/**
 * 三维极坐标转直角坐标
 * 极坐标：(r, theta, phi) -> 直角坐标：(x, y, z)
 * theta：方位角（xy平面），phi：俯仰角（xz平面）
 */
void polar3d_to_cartesian(const RadarPoint3D *point, double *x, double *y, double *z)
{
    *x = point->r * sin(point->phi) * cos(point->theta); // x = r·sinφ·cosθ
    *y = point->r * sin(point->phi) * sin(point->theta); // y = r·sinφ·sinθ
    *z = point->r * cos(point->phi);                     // z = r·cosφ
}

/**
 * 三维直角坐标转极坐标
 * 直角坐标：(x, y, z) -> 极坐标：(r, theta, phi)
 */
void cartesian_to_polar3d(double x, double y, double z, double *r, double *theta, double *phi)
{
    *r = sqrt(x * x + y * y + z * z);     // 距离
    *theta = atan2(y, x);                 // 方位角（范围：[-π, π]）
    *phi = atan2(sqrt(x * x + y * y), z); // 俯仰角（范围：[0, π]）
}

/**
 * 3x3矩阵乘法：C = A * B
 */
void mat33_mult(const double A[3][3], const double B[3][3], double C[3][3])
{
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            C[i][j] = 0;
            for (int k = 0; k < 3; k++)
            {
                C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

/**
 * 6x6矩阵乘法：C = A * B
 */
void mat66_mult(const double A[6][6], const double B[6][6], double C[6][6])
{
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            C[i][j] = 0;
            for (int k = 0; k < 6; k++)
            {
                C[i][j] += A[i][k] * B[k][j];
            }
        }
    }
}

/**
 * 6x6矩阵转置：B = A^T
 */
void mat66_transpose(const double A[6][6], double B[6][6])
{
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            B[i][j] = A[j][i];
        }
    }
}

/**
 * 3x3矩阵求逆（针对对称矩阵，简化计算）
 * 返回值：true=成功，false=失败（行列式接近0）
 */
bool mat33_inv(const double A[3][3], double invA[3][3])
{
    // 计算行列式
    double det = A[0][0] * (A[1][1] * A[2][2] - A[1][2] * A[2][1]) - A[0][1] * (A[1][0] * A[2][2] - A[1][2] * A[2][0]) + A[0][2] * (A[1][0] * A[2][1] - A[1][1] * A[2][0]);

    if (fabs(det) < 1e-8)
        return false; // 行列式接近0，不可逆
    double inv_det = 1.0 / det;

    // 伴随矩阵 / 行列式（对称矩阵优化）
    invA[0][0] = (A[1][1] * A[2][2] - A[1][2] * A[2][1]) * inv_det;
    invA[0][1] = (A[0][2] * A[2][1] - A[0][1] * A[2][2]) * inv_det;
    invA[0][2] = (A[0][1] * A[1][2] - A[0][2] * A[1][1]) * inv_det;
    invA[1][0] = invA[0][1]; // 对称矩阵特性
    invA[1][1] = (A[0][0] * A[2][2] - A[0][2] * A[2][0]) * inv_det;
    invA[1][2] = (A[0][2] * A[1][0] - A[0][0] * A[1][2]) * inv_det;
    invA[2][0] = invA[0][2]; // 对称矩阵特性
    invA[2][1] = invA[1][2]; // 对称矩阵特性
    invA[2][2] = (A[0][0] * A[1][1] - A[0][1] * A[1][0]) * inv_det;

    return true;
}

/**
 * 初始化三维卡尔曼滤波器（匀速度模型）
 * dt：采样时间(s)，process_noise：过程噪声系数，measure_noise：测量噪声系数
 */
void kalman3d_init(KalmanFilter3D *kf, double dt, double process_noise, double measure_noise)
{
    // 状态向量初始化（x, y, z, vx, vy, vz）
    for (int i = 0; i < 6; i++)
    {
        kf->x[i] = 0.0;
    }

    // 协方差矩阵 P 初始化（对角线为初始不确定性）
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            kf->P[i][j] = (i == j) ? 100.0 : 0.0; // 初始不确定性较大
        }
    }

    // 状态转移矩阵 F（三维匀速度模型）
    // [1 0 0 dt 0 0;
    //  0 1 0 0 dt 0;
    //  0 0 1 0 0 dt;
    //  0 0 0 1 0 0;
    //  0 0 0 0 1 0;
    //  0 0 0 0 0 1]
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            kf->F[i][j] = 0.0;
        }
    }
    kf->F[0][0] = 1;
    kf->F[0][3] = dt;
    kf->F[1][1] = 1;
    kf->F[1][4] = dt;
    kf->F[2][2] = 1;
    kf->F[2][5] = dt;
    kf->F[3][3] = 1;
    kf->F[4][4] = 1;
    kf->F[5][5] = 1;

    // 观测矩阵 H（只观测x, y, z）
    // [1 0 0 0 0 0;
    //  0 1 0 0 0 0;
    //  0 0 1 0 0 0]
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            kf->H[i][j] = (i == j) ? 1.0 : 0.0;
        }
    }

    // 过程噪声协方差 Q（与dt和过程噪声系数相关）
    double dt2 = dt * dt;
    double dt3 = dt2 * dt;
    double dt4 = dt3 * dt;
    double q = process_noise; // 过程噪声系数

    // 位置噪声分量（x, y, z对应行/列）
    kf->Q[0][0] = dt4 / 4 * q;
    kf->Q[0][3] = dt3 / 2 * q;
    kf->Q[1][1] = dt4 / 4 * q;
    kf->Q[1][4] = dt3 / 2 * q;
    kf->Q[2][2] = dt4 / 4 * q;
    kf->Q[2][5] = dt3 / 2 * q;
    // 速度噪声分量（vx, vy, vz对应行/列）
    kf->Q[3][0] = dt3 / 2 * q;
    kf->Q[3][3] = dt2 * q;
    kf->Q[4][1] = dt3 / 2 * q;
    kf->Q[4][4] = dt2 * q;
    kf->Q[5][2] = dt3 / 2 * q;
    kf->Q[5][5] = dt2 * q;

    // 测量噪声协方差 R（雷达测距/测角转换后的噪声）
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            kf->R[i][j] = (i == j) ? measure_noise : 0.0;
        }
    }
}

/**
 * 卡尔曼滤波时间更新（预测）
 */
void kalman3d_predict(KalmanFilter3D *kf)
{
    // 1. 预测状态：x_pred = F * x
    double x_pred[6];
    for (int i = 0; i < 6; i++)
    {
        x_pred[i] = 0.0;
        for (int j = 0; j < 6; j++)
        {
            x_pred[i] += kf->F[i][j] * kf->x[j];
        }
    }

    // 2. 预测协方差：P_pred = F * P * F^T + Q
    double F_T[6][6];
    mat66_transpose(kf->F, F_T); // F的转置

    double F_P[6][6];
    mat66_mult(kf->F, kf->P, F_P); // F*P

    double P_pred[6][6];
    mat66_mult(F_P, F_T, P_pred); // F*P*F^T

    // 加上过程噪声Q
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            P_pred[i][j] += kf->Q[i][j];
        }
    }

    // 更新状态和协方差
    for (int i = 0; i < 6; i++)
    {
        kf->x[i] = x_pred[i];
        for (int j = 0; j < 6; j++)
        {
            kf->P[i][j] = P_pred[i][j];
        }
    }
}

/**
 * 卡尔曼滤波量测更新（校正）
 * z_x, z_y, z_z：测量值（直角坐标）
 */
void kalman3d_update(KalmanFilter3D *kf, double z_x, double z_y, double z_z)
{
    // 1. 测量残差：y = z - H*x
    double y[3] = {
        z_x - (kf->H[0][0] * kf->x[0] + kf->H[0][1] * kf->x[1] + kf->H[0][2] * kf->x[2]),
        z_y - (kf->H[1][0] * kf->x[0] + kf->H[1][1] * kf->x[1] + kf->H[1][2] * kf->x[2]),
        z_z - (kf->H[2][0] * kf->x[0] + kf->H[2][1] * kf->x[1] + kf->H[2][2] * kf->x[2])};

    // 2. 残差协方差：S = H*P*H^T + R
    double H_T[6][3]; // H的转置（6x3）
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            H_T[i][j] = kf->H[j][i];
        }
    }

    double H_P[3][6]; // H*P（3x6）
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            H_P[i][j] = 0.0;
            for (int k = 0; k < 6; k++)
            {
                H_P[i][j] += kf->H[i][k] * kf->P[k][j];
            }
        }
    }

    double H_P_Ht[3][3]; // H*P*H^T（3x3）
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            H_P_Ht[i][j] = 0.0;
            for (int k = 0; k < 6; k++)
            {
                H_P_Ht[i][j] += H_P[i][k] * H_T[k][j];
            }
        }
    }

    // 加上测量噪声R
    double S[3][3];
    for (int i = 0; i < 3; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            S[i][j] = H_P_Ht[i][j] + kf->R[i][j];
        }
    }

    // 3. 卡尔曼增益：K = P*H^T*S^{-1}
    double S_inv[3][3];
    if (!mat33_inv(S, S_inv))
        return; // S不可逆，跳过更新

    double P_Ht[6][3]; // P*H^T（6x3）
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            P_Ht[i][j] = 0.0;
            for (int k = 0; k < 6; k++)
            {
                P_Ht[i][j] += kf->P[i][k] * H_T[k][j];
            }
        }
    }

    double K[6][3]; // 卡尔曼增益（6x3）ok
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 3; j++)
        {
            K[i][j] = 0.0;
            for (int k = 0; k < 3; k++)
            {
                K[i][j] += P_Ht[i][k] * S_inv[k][j];
            }
        }
    }

    // 4. 更新状态：x = x_pred + K*y
    for (int i = 0; i < 6; i++)
    {
        kf->x[i] += K[i][0] * y[0] + K[i][1] * y[1] + K[i][2] * y[2];
    }

    // 5. 更新协方差：P = (I - K*H) * P_pred
    double I[6][6] = {0}; // 6x6单位矩阵
    for (int i = 0; i < 6; i++)
        I[i][i] = 1.0;

    double K_H[6][6]; // K*H（6x6）
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            K_H[i][j] = 0.0;
            for (int k = 0; k < 3; k++)
            {
                K_H[i][j] += K[i][k] * kf->H[k][j];
            }
        }
    }

    double I_KH[6][6]; // I - K*H
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            I_KH[i][j] = I[i][j] - K_H[i][j];
        }
    }

    double P_new[6][6];
    mat66_mult(I_KH, kf->P, P_new); // (I-KH)*P_pred

    // 更新协方差矩阵
    for (int i = 0; i < 6; i++)
    {
        for (int j = 0; j < 6; j++)
        {
            kf->P[i][j] = P_new[i][j];
        }
    }
}

/**
 * 初始化三维航迹库
 */
void track3d_init(Track3D tracks[MAX_TRACKS_3D])
{
    for (int i = 0; i < MAX_TRACKS_3D; i++)
    {
        tracks[i].id = i;
        tracks[i].life = 0;
        tracks[i].max_life = MAX_LIFE_3D;
        tracks[i].is_valid = false;
        // 初始化卡尔曼滤波器（采样时间1s，过程噪声0.1，测量噪声1.0）
        kalman3d_init(&tracks[i].kf, 1.0, 0.1, 1.0);
    }
}

/**
 * 计算三维空间中两点的欧氏距离
 */
double distance3d(double x1, double y1, double z1, double x2, double y2, double z2)
{
    return sqrt(pow(x1 - x2, 2) + pow(y1 - y2, 2) + pow(z1 - z2, 2));
}

/**
 * 三维数据关联（最近邻法+波门限制）
 * 输入：点迹数组、点迹数量、航迹库
 */
void data_association3d(RadarPoint3D *points, const int point_num, Track3D tracks[MAX_TRACKS_3D])
{
    bool assigned[MAX_TRACKS_3D] = {false}; // 标记航迹是否已匹配
    int associations[point_num];            // 点迹匹配的航迹索引（-1表示未匹配）
    for (int i = 0; i < point_num; i++)
        associations[i] = -1;

    // 1. 遍历点迹，寻找匹配的航迹（最近邻+波门限制）
    for (int i = 0; i < point_num; i++)
    {
        double x, y, z;
        polar3d_to_cartesian(&points[i], &x, &y, &z); // 点迹转直角坐标

        double min_dist = 1e9;
        int best_track = -1;

        // 遍历有效航迹，计算与预测位置的距离
        for (int j = 0; j < MAX_TRACKS_3D; j++)
        {
            if (!tracks[j].is_valid || assigned[j])
                continue;

            // 航迹预测位置（卡尔曼预测后的x, y, z）
            double pred_x = tracks[j].kf.x[0];
            double pred_y = tracks[j].kf.x[1];
            double pred_z = tracks[j].kf.x[2];

            // 计算三维欧氏距离，小于波门阈值视为潜在匹配
            double dist = distance3d(x, y, z, pred_x, pred_y, pred_z);
            if (dist < min_dist && dist < GATE_THRESHOLD)
            {
                min_dist = dist;
                best_track = j;
            }
        }

        if (best_track != -1)
        {
            associations[i] = best_track;
            assigned[best_track] = true;
        }
    }

    // 2. 更新匹配的航迹，未匹配的航迹寿命递减
    for (int j = 0; j < MAX_TRACKS_3D; j++)
    {
        if (!tracks[j].is_valid)
            continue;

        bool matched = false;
        for (int i = 0; i < point_num; i++)
        {
            if (associations[i] == j)
            {
                // 匹配成功：更新航迹
                double x, y, z;
                polar3d_to_cartesian(&points[i], &x, &y, &z);
                kalman3d_update(&tracks[j].kf, x, y, z); // 卡尔曼校正
                tracks[j].life = (tracks[j].life < MAX_LIFE_3D) ? tracks[j].life + 1 : MAX_LIFE_3D;
                matched = true;
                break;
            }
        }

        if (!matched)
        {
            // 未匹配：寿命递减
            tracks[j].life--;
            if (tracks[j].life <= 0)
            {
                tracks[j].is_valid = false; // 寿命耗尽，标记为无效
            }
        }
    }

    // 3. 为未匹配的点迹创建新航迹
    for (int i = 0; i < point_num; i++)
    {
        if (associations[i] == -1)
        {
            // 寻找空闲航迹ID
            int free_idx = -1;
            for (int j = 0; j < MAX_TRACKS_3D; j++)
            {
                if (!tracks[j].is_valid)
                {
                    free_idx = j;
                    break;
                }
            }
            if (free_idx == -1)
                continue; // 航迹库已满

            // 初始化新航迹
            double x, y, z;
            polar3d_to_cartesian(&points[i], &x, &y, &z);
            tracks[free_idx].kf.x[0] = x;     // 初始x
            tracks[free_idx].kf.x[1] = y;     // 初始y
            tracks[free_idx].kf.x[2] = z;     // 初始z
            tracks[free_idx].kf.x[3] = 0;     // 初始vx
            tracks[free_idx].kf.x[4] = 0;     // 初始vy
            tracks[free_idx].kf.x[5] = 0;     // 初始vz
            tracks[free_idx].life = 1;        // 初始寿命
            tracks[free_idx].is_valid = true; // 标记为有效
        }
    }
}

/**
 * 处理一帧三维雷达数据（主流程）
 */
void radar3d_track_process(RadarPoint3D *points, int point_num, Track3D tracks[MAX_TRACKS_3D])
{
    // 1. 对所有有效航迹进行预测
    for (int i = 0; i < MAX_TRACKS_3D; i++)
    {
        if (tracks[i].is_valid)
        {
            kalman3d_predict(&tracks[i].kf);
        }
    }

    // 2. 数据关联与航迹更新
    data_association3d(points, point_num, tracks);

    // 3. 输出有效航迹状态
    printf("=== track result ===\n");
    for (int i = 0; i < MAX_TRACKS_3D; i++)
    {
        if (tracks[i].is_valid)
        {
            printf("track ID: %d\n", tracks[i].id);
            printf("  position: (%.2f, %.2f, %.2f) m\n",
                   tracks[i].kf.x[0], tracks[i].kf.x[1], tracks[i].kf.x[2]);
            printf("  speed: (%.2f, %.2f, %.2f) m/s\n",
                   tracks[i].kf.x[3], tracks[i].kf.x[4], tracks[i].kf.x[5]);
            printf("  life: %d\n", tracks[i].life);
            printf("\n\n");
        }
    }
}

int main()
{
    // 初始化三维航迹库
    Track3D tracks[MAX_TRACKS_3D];
    track3d_init(tracks);

    // 模拟3帧雷达数据（两个目标：三维空间移动）
    // 第1帧：目标A(10m, 30°, 15°)，目标B(20m, 60°, 30°)
    RadarPoint3D frame1[2] = {
        {10.0, M_PI / 6, M_PI / 12, 1000}, // 30°=π/6, 15°=π/12
        {20.0, M_PI / 3, M_PI / 6, 1000}   // 60°=π/3, 30°=π/6
    };

#if 0
    for(int i=0;i<2;i++){
       double x1, y1, z1; 
       polar3d_to_cartesian(&frame1[i],&x1,&y1,&z1);
       printf("---------------------------------------------simulate data x1=%d\t y1=%d\t z1=%d\n-----------------------------------------", (int)x1, (int)y1, (int)z1);

    }
#endif
    // double x1, y1, z1;
    // polar3d_to_cartesian(frame1,&x1,&y1,&z1);
    // printf("simulate data x1=%d\t y1=%d\t z1=%d\n", (int)x1, (int)y1, (int)z1);
    printf("---the first frame data---\n");
    // printf("--------------simulate data x1=%d\t y1=%d\t z1=%d\n-----------------", (int)x1, (int)y1, (int)z1);
    for (int i = 0; i < 2; i++)
    {
        double x1, y1, z1;
        polar3d_to_cartesian(&frame1[i], &x1, &y1, &z1);
         printf("----------------------simulate data x1=%.2f\t y1=%.2f\t z1=%.2f----------------------\n", x1, y1, z1);
    }
    radar3d_track_process(frame1, 2, tracks);
    // 第2帧：目标A移动到(11m, 30°, 15°)，目标B移动到(22m, 60°, 30°)
    RadarPoint3D frame2[2] = {
        {11.0, M_PI / 6, M_PI / 12, 2000},
        {22.0, M_PI / 3, M_PI / 6, 2000}};
    // double x2, y2, z2;
    // polar3d_to_cartesian(frame2,&x2,&y2,&z2);
    // printf("simulate data x2=%d\t y2=%d\t z2=%d\n", (int)x2, (int)y2, (int)z2);
    printf("\n--- the second frame data---\n");
    for (int i = 0; i < 2; i++)
    {
        double x1, y1, z1;
        polar3d_to_cartesian(&frame2[i], &x1, &y1, &z1);
        printf("----------------------simulate data x1=%.2f\t y1=%.2f\t z1=%.2f----------------------\n", x1, y1, z1);
    }
    radar3d_track_process(frame2, 2, tracks);
    // 第3帧：目标A移动到(12m, 30°, 15°)，目标B消失
    RadarPoint3D frame3[1] = {
        {12.0, M_PI / 6, M_PI / 12, 3000}};
    // double x3, y3, z3;
    // polar3d_to_cartesian(frame3,&x3,&y3,&z3);
    printf("\n--- the third frame data ---\n");
    for (int i = 0; i < 2; i++)
    {
        double x1, y1, z1;
        polar3d_to_cartesian(&frame3[i], &x1, &y1, &z1);
        printf("----------------------simulate data x1=%.2f\t y1=%.2f\t z1=%.2f----------------------\n", x1, y1, z1);
    }
    radar3d_track_process(frame3, 1, tracks);
    return 0;
}